#!/usr/bin/env python3
import rospy
import math
import time
from std_msgs.msg import String

class STM32SimulatorSimple:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('stm32_simulator', anonymous=True)
        
        # 模拟参数
        self.rate = rospy.Rate(10)  # 10Hz
        self.amplitude = 180.0  # 偏航角范围 ±180度
        
        # 发布模拟的串口数据
        self.serial_pub = rospy.Publisher('/simulated_serial_data', String, queue_size=10)
        
        # 模拟开始时间
        self.start_time = time.time()
        
        rospy.loginfo("精简版STM32模拟器已启动")
        
    def generate_sine_wave(self, elapsed_time, frequency=0.1):
        """生成正弦波偏航数据"""
        # 生成-180°到+180°的正弦波
        yaw = self.amplitude * math.sin(2 * math.pi * frequency * elapsed_time)
        return yaw
    
    def run(self):
        """主循环"""
        while not rospy.is_shutdown():
            try:
                # 计算经过的时间
                elapsed_time = time.time() - self.start_time
                
                # 生成偏航数据
                yaw = self.generate_sine_wave(elapsed_time)
                
                # 格式化数据为 "YAW:123.45" 格式
                serial_data = "YAW:{:.2f}".format(yaw)
                
                # 发布模拟串口数据
                serial_msg = String()
                serial_msg.data = serial_data
                self.serial_pub.publish(serial_msg)
                
                rospy.loginfo("发送: {}".format(serial_data))
                
            except Exception as e:
                rospy.logerr("模拟数据生成错误: {}".format(e))
                
            self.rate.sleep()

if __name__ == '__main__':
    try:
        simulator = STM32SimulatorSimple()
        simulator.run()
    except rospy.ROSInterruptException:
        pass
